//send udp
#include "opencv2/opencv.hpp"
#include <iostream>
#include <string>
#include <sys/time.h>

#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

#include <sys/types.h>
#include <sys/socket.h>
#include<pthread.h>
#include <netinet/in.h>
#include <arpa/inet.h>  
using namespace cv;
using namespace std;
using namespace dnn;

#define err_sys(msg) \
	do { perror(msg); exit(-1); } while(0)
#define err_exit(msg) \
	do { fprintf(stderr, msg); exit(-1); } while(0)


#define MICRO_IN_SEC 1000000.00

double microtime();
int ssd();

void *thread_cv_img(void *arg);
void *thread_detec(void *arg);

Mat frame;
Mat imageOut;
int frameOK = 0;
int detecOK = 0;

void send_float(float x, float y);

void *thread_cv_img(void *arg)
{
	VideoCapture cap("rtsp://admin:12345@192.168.3.3:8554/live");
	while (1)
	{
		frameOK = 0;
		cap >> frame;
		if (!frame.empty())
		{
			frameOK = 1;
			imshow("capture", frame);
			waitKey(1);
		}
		if (detecOK == 1)
		{
			imshow("test", imageOut);
			waitKey(1);
			detecOK = 0;
		}
	}
}

void *thread_detec(void *arg)
{
	sleep(3);
	cout << "detec start" << endl;
	double start_time, dt, dt_err;
	start_time = microtime();
	dt_err = microtime() - start_time;
	double last_send_time = microtime();
	String prototxt = "MobileNetSSD_deploy.prototxt";
	String caffemodel = "MobileNetSSD_deploy.caffemodel";
	Net net = readNetFromCaffe(prototxt, caffemodel);

	const char* classNames[] = { "background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair",
		"cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor" };

	float detect_thresh = 0.25;
	if (true)
	{
		net.setPreferableTarget(0);
	}

	int picN = 0;
	char filename[256];
	int max_lock = 10;
	Point human_point[10];
	int human_area[10];
	float human_x, human_y;
	while (1)
	{
		start_time = microtime();
		int trigger = 0;
		if (frameOK==1)
		{
			Mat image;
			frame.copyTo(image);
			cout << "frame in" << endl;
			net.setInput(blobFromImage(image, 1.0 / 127.5, Size(300, 300), Scalar(127.5, 127.5, 127.5), true, false));
			Mat cvOut = net.forward();
			Mat detectionMat(cvOut.size[2], cvOut.size[3], CV_32F, cvOut.ptr<float>());
			for (int i = 0; i < detectionMat.rows; i++)
			{
				int obj_class = detectionMat.at<float>(i, 1);
				float confidence = detectionMat.at<float>(i, 2);

				if (confidence > detect_thresh)
				{
					size_t objectClass = (size_t)(detectionMat.at<float>(i, 1));

					int xLeftBottom = static_cast<int>(detectionMat.at<float>(i, 3) * image.cols);
					int yLeftBottom = static_cast<int>(detectionMat.at<float>(i, 4) * image.rows);
					int xRightTop = static_cast<int>(detectionMat.at<float>(i, 5) * image.cols);
					int yRightTop = static_cast<int>(detectionMat.at<float>(i, 6) * image.rows);

					ostringstream ss;
					int tmpI = 100 * confidence;
					ss << tmpI;
					String conf(ss.str());

					Rect object((int)xLeftBottom, (int)yLeftBottom,
						(int)(xRightTop - xLeftBottom),
						(int)(yRightTop - yLeftBottom));
					if (classNames[objectClass] == "person")
					{
						trigger++;
						rectangle(image, object, Scalar(0, 0, 255), 1);
						String label = String(classNames[objectClass]) + ": " + conf + "%";
						putText(image, label, Point((xLeftBottom+ xRightTop)/2, yLeftBottom + 30 * (i + 1)), 2, 0.8, Scalar(0, 0, 255), 2);
						if (trigger < max_lock)
						{
							human_point[trigger - 1].x = object.x + cvRound(object.width / 2.0);
							human_point[trigger - 1].y = object.y + cvRound(object.height / 2.0);
							human_area[trigger - 1] = object.area();
						}
					}
					else if (classNames[objectClass] == "cat" || classNames[objectClass] == "dog")
					{
						rectangle(image, object, Scalar(0, 255, 0), 1);
						String label = String(classNames[objectClass]) + ": " + conf + "%";
						putText(image, label, Point((xLeftBottom + xRightTop) / 2, yLeftBottom + 30 * (i + 1)), 2, 0.7, Scalar(0, 255, 0), 2);
					}
					else
					{
						rectangle(image, object, Scalar(255, 0, 0), 1);
						String label = String(classNames[objectClass]) + ": " + conf + "%";
						putText(image, label, Point((xLeftBottom + xRightTop) / 2, yLeftBottom + 30 * (i + 1)), 2, 0.7, Scalar(255, 0, 0), 2);
					}
				}
			}
			if (trigger >= 1)
			{
				image.copyTo(imageOut);
				detecOK = 1;
				sprintf(filename, "/home/dat/test/img/%06d.jpg", picN);
				picN++;
				if (picN > 1000)
					picN = 0;
				imwrite(filename, image);
				cout << "Waning" << endl;

				int human_n = 0;
				int human_area_tmp = 0;
				Point bighuman_point;
				for (human_n = 0; human_n < trigger; human_n++)
				{
					if (human_area[human_n] > human_area_tmp)
					{
						human_area_tmp = human_area[human_n];
						bighuman_point = human_point[human_n];
					}
				}
				human_x = (bighuman_point.x - 320)*0.075;
				human_y = (240 - bighuman_point.y)*0.075;
				cout << "location: x = " << human_x << "	y = " << human_y << endl;
				if (microtime() - last_send_time > 5)
				{
					send_float(human_x, human_y);
					last_send_time = microtime();
				}
			}
			dt = microtime() - start_time - dt_err;
			cout << "Cost time: " << dt << " s" << endl;
		}
		else
		{
			continue;
		}	
	}


}
int main(int argc, char** argv)
{
	pthread_t tid_cv_img;
	char* p_cv_img = NULL;
	pthread_create(&tid_cv_img, NULL, thread_cv_img, NULL);
	
	pthread_t tid_detec;
	char* p_detec = NULL;
	pthread_create(&tid_detec, NULL, thread_detec, NULL);

	pthread_join(tid_cv_img, (void **)&p_cv_img);
	pthread_join(tid_detec, (void **)&p_detec);
	return 0;
}
double microtime() {

	struct timeval tv;
	struct timezone tz;
	gettimeofday(&tv, &tz);
	return tv.tv_sec + tv.tv_usec / MICRO_IN_SEC;
}
void send_float(float x, float y)
{
	float *floatBuff = new float[2];//
	floatBuff[0] = x;
	floatBuff[1] = y;
	char *buff=(char *)floatBuff;//
	struct sockaddr_in addr;
	memset(&addr, 0, sizeof(struct sockaddr_in));
	addr.sin_family = AF_INET;
	addr.sin_port = htons(6000);
	//   addr.sin_addr.s_addr = htonl(INADDR_ANY);
	addr.sin_addr.s_addr = inet_addr("127.0.0.1");
	int sock;
	sock = socket(AF_INET, SOCK_DGRAM, 0);
	//  bind(sock, (struct sockaddr *)&addr, sizeof(addr)); 
	sendto(sock, buff, strlen(buff)+1, 0, (struct sockaddr *)&addr, sizeof(addr));
	delete floatBuff;
	close(sock);
}